# -*- encoding: utf-8 -*-
"""
last edited at 2018-6-21
edition logs:
    2018-6-12 by Stone
    1. Added CrossRoad.laneGuides and its show and  hide method.

    2018-6-21 by Stone
    1.Added part of acceleration control and rotating feature.
    2.Those feature passed some tests (not professional test though, it just worked :) ).
"""
from Shapes import *
from visual import *

# ===== Type definition =====
pos = vector  # to convenient some pos calculation


# ===== Classes =====
class Car(VehicleShape, object):
    """This class is used to provide the geometric and dynamic features of the Cars.
    All the unit is based on SI, i.e. m/s, m/s^2"""
    def __init__(self, orientation, driver=None, init_pos=vector(100,100,-200)):
        super(Car, self).__init__(model='car')
        self.__orientation = orientation  # the unit vector indicates where the car is facing toward
        self.body.axis = self.__orientation * self.body.axis.mag
        self.set_pos(init_pos)
        self.__v = vector(0, 0, 0)
        self.__a = vector(0, 0, 0)
        self.__w = 0  # omega, angular velocity in degree

        self.driver = driver
        self.give_a_driver(driver)

    def move_one_frame(self, delta_t):
        # update status
        self.__v += self.__a * delta_t
        ang = radians(self.__w * delta_t)
        self.body.rotate(angle=ang, axis=scene.up)
        self.__orientation = self.body.axis.norm()
        self.__a = self.__orientation * self.__a.mag
        self.__v = self.__orientation * self.__v.mag
        # update positions
        self.body.pos += self.__v * delta_t

    def get_pos(self):
        return self.body.pos

    def get_orientation(self):
        return self.__orientation

    def get_a(self):
        return self.__a

    def get_v(self):
        return self.__v

    def get_w(self):
        return self.__w

    def get_face_to_face_distance(self, anotherCar):
        """To get the distance from the former car's back face to this car's front face."""
        l, w, h = anotherCar.get_size()
        delta_pos = anotherCar.get_pos() - self.__pos
        dis = sqrt(delta_pos[0]**2 + delta_pos[1]**2 + delta_pos[2]**2) - (l + self.__length)/2

        return dis

    def set_pos(self, pos):
        self.body.pos = pos
        # to keep car's buttom stay on the ground
        self.body.pos[2] = self.get_size()['height'] / 2.0

    def set_a(self, a):
        self.__a = a

    def set_orientation(self, orn):
        """The axis should be visual.vector type"""
        self.__orientation = orn

    def set_w(self, w):
        self.__w = w

    def is_it_your_driver(self, theDriver):
        return self.driver == theDriver

    def give_a_driver(self, driver):
        self.driver = driver
        if self.driver and (not self.driver.is_it_your_car(self) ):
            self.driver.give_a_car(self)


class CrossRoad(CrossRoadShape):
    """Construct the shape of the crossroad and provide some attributes of this road."""
    def __init__(self, visual_frame):
        self.f = visual_frame
        CrossRoadShape.__init__(self, self.f)

        self.ifShow_LaneGuides = True
        self.laneGuides = {'x': [], '-x': [], 'y': [], '-y': [], 'visual': []}  # used to guide the vehicles
        self.start = {'x': [], '-x': [], 'y': [], '-y': [], 'visual': []}  # where the vehicles start

        self.init_laneGuides()
        if self.ifShow_LaneGuides: self.show_laneGuides()
        #self.construct_shape()

    def init_laneGuides(self):
        num_of_lanes = 4
        lane_center_bias = self.width / 2.0
        # not clever enough, so manually specify some values intelligently
        k=2
        for i in range(1, num_of_lanes + 1):
            self.start['x'].append(( -k * self.length, -(i * self.width + lane_center_bias),0))
            self.start['-x'].append(( k * self.length,   i * self.width + lane_center_bias, 0))
            self.start['y'].append((   i * self.width + lane_center_bias,  -k * self.length, 0))
            self.start['-y'].append((-(i * self.width + lane_center_bias),  k * self.length, 0))

            # the lanes that go straight
            self.laneGuides[ 'x'].append([( k*self.length, -(i*self.width + lane_center_bias), 0)])  # the guide point
            self.laneGuides['-x'].append([(-k*self.length,   i*self.width + lane_center_bias,  0)])  # -x direction
            self.laneGuides[ 'y'].append([(  i*self.width + lane_center_bias,  k*self.length, 0)])  # y direction, too
            self.laneGuides['-y'].append([(-(i*self.width + lane_center_bias),-k*self.length, 0)])

        # the fifth lanes that turn right
        # the guide points at turn
        self.laneGuides['x'].append([(self.laneGuides['-y'][-1][0][0], -(num_of_lanes * self.width + lane_center_bias),0)])
        self.laneGuides['-x'].append([(self.laneGuides[ 'y'][-1][0][0],  num_of_lanes * self.width + lane_center_bias, 0)])
        self.laneGuides['y'].append([(   num_of_lanes * self.width + lane_center_bias,  self.laneGuides[ 'x'][-1][0][1], 0)])
        self.laneGuides['-y'].append([(-(num_of_lanes * self.width + lane_center_bias), self.laneGuides['-x'][-1][0][1], 0)])
        k = 2
        self.start[ 'x'].append((-k * self.length,-(num_of_lanes * self.width + lane_center_bias), 0))
        self.start['-x'].append(( k * self.length,  num_of_lanes * self.width + lane_center_bias,  0))
        self.start[ 'y'].append((  num_of_lanes * self.width + lane_center_bias, -k * self.length, 0))
        self.start['-y'].append((-(num_of_lanes * self.width + lane_center_bias), k * self.length, 0))
        # the end guide points
        self.laneGuides[ 'x'][-1].append((self.laneGuides['-y'][-1][0][0], -k*self.length, 0))
        self.laneGuides['-y'][-1].append((-k*self.length, self.laneGuides['-x'][-1][0][1], 0))
        self.laneGuides['-x'][-1].append((self.laneGuides[ 'y'][-1][0][0],  k*self.length, 0))
        self.laneGuides[ 'y'][-1].append(( k*self.length, self.laneGuides[ 'x'][-1][0][1], 0))
        for dirc in self.laneGuides:
            for l in self.laneGuides[dirc]:
                for i in range(len(l)):
                    l[i] = vector(l[i])
        for dirc in self.start:
            for i in range(len(self.start[dirc])):
                self.start[dirc][i] = vector(self.start[dirc][i])
        print 'laneGuides: \n', self.laneGuides
        print 'starts: \n', self.start

    def hide_laneGuides(self):
        for obj in self.laneGuides['visual']:
            obj.visible = False

    def show_laneGuides(self):
        if self.laneGuides['visual'] == []:  # time to initialize
            for i in self.laneGuides:
                if i == 'visual': continue
                for j in range(len(self.laneGuides[i])):
                    self.laneGuides['visual'].append(curve(pos=[self.start[i][j]] + self.laneGuides[i][j]))
                    # test 1
                    if j == 4: self.laneGuides['visual'][-1].color = color.yellow
                    # test 1 end
        for obj in self.laneGuides['visual']:
            obj.visible = True


class Driver:
    """Drive the car, which means put the traffic rules in this class."""
    def __init__(self, start, laneGuide, car=None):
        self.__MAX_A = (100.0/3.6) / 10    # (100km/h / 3.6)/10s, max acceleration, expressed in 100km/h speed up
        self.__MAX_V = 60/3.6              # 60 km/h / 3.6, max velocity
        self.__MAX_W = radians(30)         # 30 degrees/s, max angular velocity
        self.__DISTANCE_RANGE = (0.5, 2)   # the distance range between two cars
        self.__k_a = 0.3
        self.__k_w = -60     # right rotate
        self.__target_v = 5  # 60/3.6
        self.__laneGuide = iter(laneGuide)                   # the laneGuide at specific lane
        self.__node = {'from': vector(start), 'to': vector(laneGuide[0])}   # the last node where it from and the next one where it's going to
        self.__target_orientation = self.get_target_orientation()

        self.car = car
        self.give_a_car(car)

    def drive(self, delta_t=0):
        self.control_distance()
        self.control_v()
        self.control_orientation()

        self.limit_a()
        self.limit_w()

        self.car.move_one_frame(delta_t)

    def control_distance(self):
        pass

    def control_v(self):
        delta_v = self.__target_v - self.car.get_v().mag
        self.car.set_a(self.__k_a * delta_v * self.car.get_orientation())

    def control_orientation(self):
        w = self.__k_w * signed_diff_angle(self.car.get_orientation(), self.__target_orientation)
        self.car.set_w(w)
        # debug
        print "tar_orn:", self.__target_orientation,

    def update_node(self):
        self.__node['from'] = self.__node['to']
        try:
            self.__node['to']   = vector(self.__laneGuide.next())
        except StopIteration:
            # now we get the destination, time to say bye bye
            self.say_goodbye()

    def get_target_orientation(self):
        return (self.__node['to'] - self.__node['from']).norm()

    def say_goodbye(self):pass

    def limit_a(self):
        if self.car.get_a().mag > self.__MAX_A:
            self.car.set_a(self.__MAX_A * self.car.get_orientation())

    def limit_w(self):
        if self.car.get_w() > self.__MAX_W:
            self.car.set_w(self.__MAX_W)

    def is_it_your_car(self, theCar):
        return self.car == theCar

    def give_a_car(self, car):
        self.car = car
        if self.car and (not self.car.is_it_your_driver(self)):
            self.car.give_a_driver(self)


# ===== Datas and Initialization =====
scene = display(title=u"十字路口模拟器2018 - by teleM".encode("gbk"))
scene.fov = pi/8
scene.forward = (0, -1, -1)
scene.up = (0, 0, 1)
crd = CrossRoad(frame())

car_groups = dict([(k, []) for k in crd.laneGuides])

framerate = 10
dt = 1.0 / framerate


# ===== Functions =====
def signed_diff_angle(v1, v2, ref_axis=scene.up):
    """  To calculate the angle with sign of two vectors,
    if v1 is clockwise to v2, then get a positive angle,
    if v1 is counter-clockwise to v2, then get a negative angle.
    The sign is given by the direction related to the ref_axis, which points outward to paper.
    This function is not prepared for every conditions, and only designed for where the refer axis is perpendicular
    to the plane that  v1, v2 lie."""
    relative_ang = diff_angle(cross(v1, v2), ref_axis)
    if relative_ang > pi/2.0:
        ang = diff_angle(v1, v2)
    elif relative_ang < pi/2.0:
        ang = -diff_angle(v1, v2)
    else:
        ang = 0  # v1 is parallel to v2
    print "v1:%s, v2:%s, ang:%.4f, rel_ang:%s cr_axis:%s, ref_axis:%s|" % (v1, v2, ang, relative_ang, cross(v1, v2), ref_axis),  # debug

    return ang


# test
arrow(axis=(5, 0, 0), color=(1, 0, 0)), arrow(axis=(0,5,0), color=(0,1,0)), arrow(axis=(0,0,5), color=(0,0,1))
car_groups['x'].append(Car(orientation=vector(1, 0, 0), init_pos=crd.start['x'][4],
                           driver=Driver(crd.start['x'][4], crd.laneGuides['x'][4])))  # debug
c = car_groups['x'][0]


def test():
    car_groups['x'][0].driver.drive(dt)
    print " | orn: %s, a:%s, v:%s, w:%s, dt:%.2f" % (c.get_orientation(), c.get_a(), c.get_v(), c.get_w(), dt)
    if c.body.pos.mag > crd.start['x'][0].mag:
        c.set_pos(crd.start['x'][0])

    rate(framerate)

# ===== Main Loop =====
# ... ...

if __name__ == '__main__':
    while 1:
       test()
